// Have include guard.
#ifndef XBOXDIVECONTROLLER_H
#define XBOXDIVECONTROLLER_H

// Include files.
#include <vector>
#include "WPILib.h"
#include "Modifier.hpp"
#include "RobotController.hpp"
#include "XboxJoystick.hpp"

template
<
	typename Drive, 
	typename Value, 
	Value(*Fetcher)(XboxJoystick &joystick),
	void(*Handler)(Drive &drive, Value &value)
>
class XboxDriveController:
	public RobotController
{
	public:
		typedef std::vector<Modifier<Value>*> ModiferList;
	protected:
		Drive &m_drive;
		XboxJoystick &m_joystick;
		ModiferList m_modifiers;
	public:
		XboxDriveController(Drive &drive, XboxJoystick &joystick) :
			m_drive(drive),
			m_joystick(joystick),
			m_modifiers()
		{
		}
		
		ModiferList &GetModifiers()
		{
			return m_modifiers;
		}
		
		const ModiferList &GetModifiers() const
		{
			return m_modifiers;
		}
		
	protected:
		virtual void Periodic()
		{
			Value value((Fetcher(m_joystick)));
			
			typename ModiferList::iterator it;
			
			for (it = m_modifiers.begin(); it != m_modifiers.end(); ++it)
				(*it)->Modify(value);
			
			Handler(m_drive, value);
		}
		
		virtual void DeactivateImpl()
		{
			m_drive.Stop();
		}
};

#endif
